#include "Flocking.h"
#include "PhysicsUtils.h"
#include "GameObject.h"
#include "Transformation.h"
#include "Animation.h"
#include "Exception.h"
#include "Logger.h"

#include <cmath>

using namespace game_engine;
using namespace game_engine::components;
using namespace game_engine::library;
using namespace game_engine::utils;
using namespace game_engine::physics;

const Point Flocking::NULL_POSITION = Point(-1.0f, -1.0f);

///////////////////////////////////////////////////////////////////////////////
// Constructors and Destructors
///////////////////////////////////////////////////////////////////////////////

Flocking::Flocking(void)
    : SimpleBehaviour(), _currentSpeed(), _nextSpeed(), _collisionTweak(), 
    _player(NULL), _goalPosition(Flocking::NULL_POSITION)
{
    type = "Flocking";
    _others.clear();
    _obstacles.clear();
    _animationId.clear();
}

Flocking::Flocking(GameObject *obj)
    : SimpleBehaviour(obj), _currentSpeed(), _nextSpeed(), _collisionTweak(), 
    _player(NULL), _goalPosition(Flocking::NULL_POSITION)
{
    type = "Flocking";
    _others.clear();
    _obstacles.clear();
    _animationId.clear();
}

Flocking::~Flocking(void)
{
}

///////////////////////////////////////////////////////////////////////////////
// Other public methods
///////////////////////////////////////////////////////////////////////////////

void Flocking::onEnter(void) throw(...)
{
    // Check that all not NULL animations are defined into an
    // Animation component of the same object
    Animation *anim = dynamic_cast<Animation*> (getComponent(object, "Animation"));
    vector<string> list;

    map<Direction,string>::const_iterator j;
    for (j=_animationId.begin(); j!=_animationId.end(); j++)
    {
        list.push_back((*j).second);
    }
    
    vector<string>::iterator it;
    for (it=list.begin(); it<list.end(); it++)
    {
        if ((!(*it).empty()) && (!anim->sequenceExists(*it)))
        {
            string err("Animation ");
            err.append(*it);
            err.append(" not defined in Animation component");
            Logger::ERR(GAMEENGINE_LOG, err);
            throw NoSuchElementException(err);
        }
    }

    // Reset sequence time
    anim->resetSequence();
    anim->setActiveSequence(animationId(IDLE));
}

void Flocking::onLoop(const FLOAT32 time) throw(...)
{
    // Calculate next speed vector according to flocking algorithm
    flockAgainst();

    // Calculate next speed vector according to relative position from player
    avoid();

    // Calculate next speed vector according to relative position from goal
    if (_goalPosition != Flocking::NULL_POSITION)
    {
        goal();
    }

    // Compute next position and check if destination has been reached
    BOOL destReached = setNewPosition(REFRESH_TIME);
    if (destReached)
    {
        // Set sequence to IDLE
        Animation *anim = dynamic_cast<Animation*> (getComponent(object, "Animation"));
        anim->resetSequence();
        anim->setActiveSequence(animationId(IDLE));

        // Emit signal
        _goalReached();
    }
}

void Flocking::onExit(void) throw(...)
{
}

boost::signals2::connection Flocking::connect(
    Flocking::signal_void_void_t &publisher, 
    const Flocking::signal_void_void_t::slot_type &subscriber)
{
    return publisher.connect(subscriber);
}

///////////////////////////////////////////////////////////////////////////////
// Private methods
///////////////////////////////////////////////////////////////////////////////

void Flocking::flockAgainst(void) throw(...)
{
    UINT32 neighbourCount = 0;
    UINT32 velCount = 0;
    Point avgPosition;
    Point avgVelocity;
    _collisionTweak = Point(0.0f, 0.0f);

    // Initialize next speed vector
    _nextSpeed = PhysicsUtils::scale(_currentSpeed, 0.95f);

    // Short range repulsion, avoid neighbours
    FLOAT32 nearestDist = 0.0f;
    GameObject *nearestObject = NULL;
    vector<GameObject*>::iterator it;
    for (it=_others.begin(); it<_others.end(); it++)
    {
        // Check for identity
        if ((*it) == object)
        {
            continue;
        }

        FLOAT32 dist = PhysicsUtils::distSQ(getObjectPosition(object), getObjectPosition((*it)));
        if((dist < nearestDist) || (nearestDist == 0))
        {
            nearestDist = dist;
            nearestObject = (*it);
        }

        if(dist < (64.0f * 64.0f))
        {
            velCount++;
            Point otherSpeed = getObjectSpeed((*it));
            avgVelocity.setX(avgVelocity.getX() + otherSpeed.getX());
            avgVelocity.setY(avgVelocity.getY() + otherSpeed.getY());
        }

        // Having a cutoff for neighbour detection lets us split up groups of 
        // objects, and each split group will flock with itself
        if(dist < (4*4*32.0f*32.0f))
        {
            neighbourCount++;
            Point otherPosition = getObjectPosition((*it));
            avgPosition.setX(avgPosition.getX() + otherPosition.getX());
            avgPosition.setY(avgPosition.getY() + otherPosition.getY());
        }
    }  // for (it=_others.begin(); it<_others.end(); it++)
    
	FLOAT32 objRadius = 32.0f;
	if((nearestDist < (objRadius*objRadius)) && (nearestObject != NULL))
    {
		// Push away from nearest object
        Point away = PhysicsUtils::toward(getObjectPosition(nearestObject), getObjectPosition(object));
        FLOAT32 overlap = objRadius - sqrt(nearestDist);
		_collisionTweak = PhysicsUtils::scale(away, overlap*0.25f);
	}

	// Attract to average position
    // Make the sum into an actual average
    if (neighbourCount > 0)
    {
	    avgPosition = PhysicsUtils::scale(avgPosition, 1.0f/neighbourCount); 
	    Point to = PhysicsUtils::toward(getObjectPosition(object), avgPosition);
	    FLOAT32 dist = PhysicsUtils::dist(getObjectPosition(object), avgPosition);
	    if ((dist > 34.0f) && (dist < 128.0f))
        {
		    Point accel = PhysicsUtils::scale(to, 1.0f*sqrt(dist-32.0f));
            _nextSpeed.setX(_nextSpeed.getX() + accel.getX());
            _nextSpeed.setY(_nextSpeed.getY() + accel.getY());
	    }
    }

	// Attract to neighbours' velocity
	if(velCount > 0)
    {
        // Average velocity
		avgVelocity = PhysicsUtils::scale(avgVelocity, 1.0f/velCount); 
		// Take weighted average between avgVelocity and nextSpeed
        Point temp = PhysicsUtils::scale(_nextSpeed, 10.0f);
        avgVelocity.setX(avgVelocity.getX() + temp.getX());
        avgVelocity.setY(avgVelocity.getY() + temp.getY());
		_nextSpeed = PhysicsUtils::scale(avgVelocity, 1.0f/11.0f);
	}

}

void Flocking::avoid(void) throw(...)
{
    // Avoid player
    Point away = PhysicsUtils::toward(getObjectPosition(_player), getObjectPosition(object));
    FLOAT32 dist = PhysicsUtils::dist(getObjectPosition(_player), getObjectPosition(object));
    if (dist < 64.0f)
    {
        Point accel = PhysicsUtils::scale(away, 750.0f/dist);
        _nextSpeed.setX(_nextSpeed.getX() + accel.getX());
        _nextSpeed.setY(_nextSpeed.getY() + accel.getY());
    }

    // Avoid obstacles
    vector<GameObject*>::iterator it;
    for (it=_obstacles.begin(); it<_obstacles.end(); it++)
    {
        Point away = PhysicsUtils::toward(getObjectPosition((*it)), getObjectPosition(object));
        FLOAT32 dist = PhysicsUtils::dist(getObjectPosition((*it)), getObjectPosition(object));
        if (dist < 128.0f)
        {
            Point accel = PhysicsUtils::scale(away, 900.0f/dist);
            _nextSpeed.setX(_nextSpeed.getX() + accel.getX());
            _nextSpeed.setY(_nextSpeed.getY() + accel.getY());
        }
    }
}

void Flocking::goal(void) throw(...)
{
    Point away = PhysicsUtils::toward(_goalPosition, getObjectPosition(object));
    FLOAT32 dist = PhysicsUtils::dist(_goalPosition, getObjectPosition(object));
    if (dist < 30.0f)
    {
        Point accel = PhysicsUtils::scale(away, 300.0f/dist);
        _nextSpeed.setX(_nextSpeed.getX() + accel.getX());
        _nextSpeed.setY(_nextSpeed.getY() + accel.getY());
    }
	//if((dist > 34.0f) && (dist < 128.0f))
    if(dist > 34.0f)
    {
        Point accel = PhysicsUtils::scale(away, -0.15f*(dist-32));
        _nextSpeed.setX(_nextSpeed.getX() + accel.getX());
        _nextSpeed.setY(_nextSpeed.getY() + accel.getY());
    }
}

const BOOL Flocking::setNewPosition(const FLOAT32 time) throw(...)
{
    // Obtain Transformation component
    Transformation *transf;
    transf = dynamic_cast<Transformation*> (object->getComponent("Transformation"));

    // Compute new position
    Point newPosition;
    _currentSpeed = PhysicsUtils::scale(_nextSpeed, time);
    newPosition.setX(transf->getXPosition() + _currentSpeed.getX() + _collisionTweak.getX());
    newPosition.setY(transf->getYPosition() + _currentSpeed.getY() + _collisionTweak.getY());

    // Update Transformation component
    transf->setPosition(newPosition);

    // Check if destination has been reached
    BOOL arrived = false;
    if (_goalPosition != Flocking::NULL_POSITION)
    {
        if (PhysicsUtils::nearTo(newPosition, _goalPosition, 35.0f))
        {
            arrived = true;
        }
    }
    return arrived;
}

void Flocking::setNewSequence(void) throw (...)
{
    // Obtain Transformation component
    Transformation *transf;
    transf = dynamic_cast<Transformation*> (object->getComponent("Transformation"));

    // Compute angle of trajectory
    FLOAT32 angle = PhysicsUtils::computeAngle(transf->getPosition(), _goalPosition);
    Direction direction = getDirectionFromAngle(angle);

    // Obtain animation component
    Animation *anim;
    anim = dynamic_cast<Animation*> (object->getComponent("Animation"));

    // Retrieve current animation sequence ID
    string seqID = anim->getActiveSequence();
    
    // Retrieve new direction animation sequence ID
    string newSeqID = animationId(direction);

    // Compare both
    if (!newSeqID.empty() && (seqID != newSeqID))
    {
        anim->resetSequence();
        anim->setActiveSequence(newSeqID);
    }
}

const Direction Flocking::getDirectionFromAngle(const FLOAT32 a) const
{
    Direction direction = IDLE;
    FLOAT32 angle = a;

    if (angle < 0.0f)
    {
        angle = 360.0f + angle;
    }
    if (angle == 0.0f)
    {
        direction = IDLE;
    }
    else if (((angle > 0.0f) && (angle < 45.0f)) || (angle > 315.0f))
    {
        direction = EAST;
    }
    else if ((angle > 45.0f) && (angle <= 135.0f))
    {
        direction = NORTH;
    }
    else if ((angle > 135.0f) && (angle <= 225.0f))
    {
        direction = WEST;
    }
    else if ((angle > 225.0f) && (angle <= 315.0f))
    {
        direction = SOUTH;
    }

    return direction;
}

Point Flocking::getObjectPosition(GameObject *obj) throw(...)
{
    Transformation *objTransf;
    try
    {
        objTransf = dynamic_cast<Transformation*> (obj->getComponent("Transformation"));
    } catch (...)
    {
        string err("getObjectPosition : transformation component not found");
        Logger::ERR(GAMEENGINE_LOG, err);
        throw NoSuchElementException(err);
    }

    return objTransf->getPosition();
}

Point Flocking::getObjectSpeed(GameObject *obj) throw(...)
{
    Flocking *objFlocking;
    try
    {
        objFlocking = dynamic_cast<Flocking*> (obj->getComponent("Flocking"));
    } catch (...)
    {
        string err("getObjectSpeed : Flocking component not found");
        Logger::ERR(GAMEENGINE_LOG, err);
        throw NoSuchElementException(err);
    }

    return objFlocking->currentSpeed();
}

///////////////////////////////////////////////////////////////////////////////
// Getters
///////////////////////////////////////////////////////////////////////////////

const Point Flocking::currentSpeed(void) const
{
    return _currentSpeed;
}

const string Flocking::animationId(const Direction direction) const
{
    // Check for existance
    map<Direction,string>::const_iterator it;
    it = _animationId.find(direction);
    if (it == _animationId.end())
    {
        return string();
    }
    return (*it).second;
}

Flocking::signal_void_void_t& Flocking::goalReached(void)
{
    return _goalReached;
}

///////////////////////////////////////////////////////////////////////////////
// Setters
///////////////////////////////////////////////////////////////////////////////

void Flocking::setOthers(const vector<GameObject*> others)
{
    _others = others;
}

void Flocking::setPlayer(GameObject *player)
{
    _player = player;
}

void Flocking::setObstacles(const vector<GameObject*> obstacles)
{
    _obstacles = obstacles;
}

void Flocking::setGoalPosition(const Point &goalPosition)
{
    _goalPosition = goalPosition;
    setNewSequence();
}

void Flocking::setAnimationId(const Direction direction, const string &animId)
{
    _animationId[direction] = animId;
}
